#include "uart.h"
#include "sysregs.h"
#include "esr.h"
#include "irq.h"
#include "timer.h"
#include "asm/base.h"
#include "mm.h"
#include "type.h"
#include "task.h"
#include "sched.h"
#include "virt_mmu.h"

typedef unsigned int size_t;
extern void virt_main(void);
extern void memcpy_test();
extern void atomic_set(unsigned long a, unsigned long b);
extern void* memset(char *s, int c, size_t count);
extern void trigger_alignment();
extern unsigned long volatile cacheline_aligned jiffies;
extern char idmap_pg_dir[];

#define read_sysreg(reg) ({ \
		unsigned long _val; \
		asm volatile("mrs %0," #reg \
		: "=r"(_val)); \
		_val; \
})


static const char *data_fault_code[] = {
	[0] = "Address size fault, level0",
	[1] = "Address size fault, level1",
	[2] = "Address size fault, level2",
	[3] = "Address size fault, level3",
	[4] = "Translation fault, level0",
	[5] = "Translation fault, level1",
	[6] = "Translation fault, level2",
	[7] = "Translation fault, level3",
	[9] = "Access flag fault, level1",
	[10] = "Access flag fault, level2",
	[11] = "Access flag fault, level3",
	[13] = "Permission fault, level1",
	[14] = "Permission fault, level2",
	[15] = "Permission fault, level3",
	[0x21] = "Alignment fault",
	[0x35] = "Unsupported Exclusive or Atomic access",
};

static const char *esr_get_dfsc_string(unsigned int esr)
{
	return data_fault_code[esr & 0x3f];
}

static const char *esr_class_str[] = {
	[0 ... ESR_ELx_EC_MAX]		= "UNRECOGNIZED EC",
	[ESR_ELx_EC_UNKNOWN]		= "Unknown/Uncategorized",
	[ESR_ELx_EC_WFx]		= "WFI/WFE",
	[ESR_ELx_EC_CP15_32]		= "CP15 MCR/MRC",
	[ESR_ELx_EC_CP15_64]		= "CP15 MCRR/MRRC",
	[ESR_ELx_EC_CP14_MR]		= "CP14 MCR/MRC",
	[ESR_ELx_EC_CP14_LS]		= "CP14 LDC/STC",
	[ESR_ELx_EC_FP_ASIMD]		= "ASIMD",
	[ESR_ELx_EC_CP10_ID]		= "CP10 MRC/VMRS",
	[ESR_ELx_EC_CP14_64]		= "CP14 MCRR/MRRC",
	[ESR_ELx_EC_ILL]		= "PSTATE.IL",
	[ESR_ELx_EC_SVC32]		= "SVC (AArch32)",
	[ESR_ELx_EC_HVC32]		= "HVC (AArch32)",
	[ESR_ELx_EC_SMC32]		= "SMC (AArch32)",
	[ESR_ELx_EC_SVC64]		= "SVC (AArch64)",
	[ESR_ELx_EC_HVC64]		= "HVC (AArch64)",
	[ESR_ELx_EC_SMC64]		= "SMC (AArch64)",
	[ESR_ELx_EC_SYS64]		= "MSR/MRS (AArch64)",
	[ESR_ELx_EC_IMP_DEF]		= "EL3 IMP DEF",
	[ESR_ELx_EC_IABT_LOW]		= "IABT (lower EL)",
	[ESR_ELx_EC_IABT_CUR]		= "IABT (current EL)",
	[ESR_ELx_EC_PC_ALIGN]		= "PC Alignment",
	[ESR_ELx_EC_DABT_LOW]		= "DABT (lower EL)",
	[ESR_ELx_EC_DABT_CUR]		= "DABT (current EL)",
	[ESR_ELx_EC_SP_ALIGN]		= "SP Alignment",
	[ESR_ELx_EC_FP_EXC32]		= "FP (AArch32)",
	[ESR_ELx_EC_FP_EXC64]		= "FP (AArch64)",
	[ESR_ELx_EC_SERROR]		= "SError",
	[ESR_ELx_EC_BREAKPT_LOW]	= "Breakpoint (lower EL)",
	[ESR_ELx_EC_BREAKPT_CUR]	= "Breakpoint (current EL)",
	[ESR_ELx_EC_SOFTSTP_LOW]	= "Software Step (lower EL)",
	[ESR_ELx_EC_SOFTSTP_CUR]	= "Software Step (current EL)",
	[ESR_ELx_EC_WATCHPT_LOW]	= "Watchpoint (lower EL)",
	[ESR_ELx_EC_WATCHPT_CUR]	= "Watchpoint (current EL)",
	[ESR_ELx_EC_BKPT32]		= "BKPT (AArch32)",
	[ESR_ELx_EC_VECTOR32]		= "Vector catch (AArch32)",
	[ESR_ELx_EC_BRK64]		= "BRK (AArch64)",
};

#define NOOP 10000

static void test_cache(void){
	unsigned long start = jiffies;
	unsigned long page;
	int i, j;
	char var;
	page = get_free_page();
	for(j = 0; j < NOOP; j++){
		flush_cache_range(page, page + PAGE_SIZE - 1);
		for (i = 0; i < PAGE_SIZE; i++){
			var = *((char *)(page + i));
		}
	}
	printk("%s time %llu jiffies\n", __func__, jiffies - start);
}

void atomatic_test(){
	unsigned long input = 0;
	atomic_set(3, &input);
}

static const char * const bad_mode_handler[] = {
	"Sync Abort",
	"IRQ",
	"FIQ",
	"SError"
};

void trigger_undef_instruction(void)
{
    // 手动构造指令（示例）
    asm volatile (
        ".word 0x96000061\n"  // 直接使用目标值作为指令
    );
}

static void parse_esr(unsigned int esr)
{
	unsigned int ec = ESR_ELx_EC(esr);

	printk("ESR info:\n");
	printk("  ESR = 0x%08x\n", esr);
	printk("  Exception class = %s, IL = %u bits\n",
		 esr_class_str[ESR_ELx_EC(esr)],
		 (esr & ESR_ELx_IL) ? 32 : 16);


	if (ec == ESR_ELx_EC_DABT_LOW || ec == ESR_ELx_EC_DABT_CUR) {
		printk("  Data abort:\n");
		if ((esr & ESR_ELx_ISV)) {
			printk("  Access size = %u byte(s)\n",
			 1U << ((esr & ESR_ELx_SAS) >> ESR_ELx_SAS_SHIFT));
			printk("  SSE = %lu, SRT = %lu\n",
			 (esr & ESR_ELx_SSE) >> ESR_ELx_SSE_SHIFT,
			 (esr & ESR_ELx_SRT_MASK) >> ESR_ELx_SRT_SHIFT);
			printk("  SF = %lu, AR = %lu\n",
			 (esr & ESR_ELx_SF) >> ESR_ELx_SF_SHIFT,
			 (esr & ESR_ELx_AR) >> ESR_ELx_AR_SHIFT);
		}
		
		printk("  SET = %lu, FnV = %lu\n",
			(esr >> ESR_ELx_SET_SHIFT) & 3,
			(esr >> ESR_ELx_FnV_SHIFT) & 1);
		printk("  EA = %lu, S1PTW = %lu\n",
			(esr >> ESR_ELx_EA_SHIFT) & 1,
			(esr >> ESR_ELx_S1PTW_SHIFT) & 1);
		printk("  CM = %lu, WnR = %lu\n",
		 (esr & ESR_ELx_CM) >> ESR_ELx_CM_SHIFT,
		 (esr & ESR_ELx_WNR) >> ESR_ELx_WNR_SHIFT);
		printk("  DFSC = %s\n", esr_get_dfsc_string(esr));
	}
}

static void kernel_panic(){
	printk("kernel panic \n");
	while(1)
		;
}


void bad_mode(struct pt_regs *regs, int reason, unsigned int esr)
{
    unsigned long current_el, far = 0;
    
    current_el = (read_sysreg(CurrentEL) >> 2) & 0x3;

    printk("Current exception level is EL%lu\n", current_el);

    // 根据异常级别选择合适的FAR寄存器
#ifdef CONFIG_VIRT
    far = read_sysreg(far_el2);
#else
	far = read_sysreg(far_el1);
#endif

    printk("Bad mode for %s handler detected, far:0x%lx esr:0x%x, ec class:0x%x\n",
            bad_mode_handler[reason], far, esr, ESR_ELx_EC(esr));
    parse_esr(esr);
    kernel_panic();
}

static void test_sysregs(void)
{
	unsigned long el;

	el = read_sysreg(CurrentEL);
	printk("el = %d\n", el >> 2);

	//write_sysreg(0x10000, vbar_el2);
	//printk("read vbar: 0x%x\n", read_sysreg(vbar_el2));
}

static int test_access_map_address(void){
	unsigned long address = TOTAL_MEMORY - 4096;

	*(unsigned long *)address = 0x55;

	printk("%s access 0x%x done\n", __func__, address);

	return 0;
}

static int test_access_unmap_address(void){
	unsigned long address = TOTAL_MEMORY + 4096;

	*(unsigned long *)address = 0x55;

	printk("%s access 0x%x done\n", __func__, address);

	return 0;

}

static void test_mmu(void){
	test_access_map_address();
	test_access_unmap_address();
}

static void delay(int n)
{
	while (n--)
		;
}

static void kernel_thread_1(void){
	while(1){
		delay(10000);
		printk("%s: %s\n", __func__, "12345");
	}
}

static void kernel_thread_2(void){
	while(1){
		delay(10000);
		printk("%s: %s\n", __func__, "678910");
	}
}

void init_main(void)
{
	paging_init();
	uart_init();
	init_printk_done();
	uart_send_string("Welcome BenOS!\r\n");
	printk("printk init done\n");

	
	//test_access_unmap_address();
	test_sysregs();
	//memcpy_test();

	printk("test------1 \n");
	dump_pgtable(idmap_pg_dir, TOTAL_MEMORY);
	//test_mmu();
	init_cache_info();

	gic_init(0, GIC_V2_DISTRIBUTOR_BASE, GIC_V2_CPU_INTERFACE_BASE);
	sched_init();


	//raw_local_irq_enable();
	//memset(0x200004, 0x55, 102);
	//atomatic_test();
	//trigger_alignment();	
	//trigger_undef_instruction();

	int pid;
	printk("test-----zhb---2 \n");
/*
	pid = do_fork(PF_KTHREAD, (unsigned long)&kernel_thread_1, 0);
	if (pid < 0)
		printk("create thread fail\n");

	pid = do_fork(PF_KTHREAD, (unsigned long)&kernel_thread_2, 0);
	if (pid < 0)
		printk("create thread fail\n");
*/
	
/*
	struct task_struct *next = g_task[pid];

	switch_to(next);
*/
/*
	timer_init();
	raw_local_irq_enable();
*/
	//test_cache();
	virt_main();
	while (1) {
		uart_send(uart_recv());
	}
}
